/*!  \file  BiMRRR.cpp
 *	  \brief  
 * 
 *   \details   details
 *   \author    Yordan Madzhunkov German Research School for Simulation Sciences GmbH
 *   \version   1.0
 *   \date      09/10/2013 02:24:36 PM
 *   \copyright GNU Public License.
 */
#include <iostream>
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <float.h>
#include "GK.h"
#include "Matrix.h"
#include "BiMRRR.h"
#include "commonSVD.h"
#include "MatrixGenerator.h"
using namespace std;

void BiMRRR::setDefaultSettings() {
	aUpper     = true;
	aUColMajor = true; 
	aVColMajor = false; 
	aVColMajor = true; 
	setCompileTime(__DATE__, __TIME__);
	gapTol       = 1e-3;
	bisRelAcc    = 1e-5; // relative accuracy for B^TB bisector
	refineRelAcc = 1e-8; // relative accuracy, before computing singletons
	maxIter = 10;
	nat = NULL;
	bis = NULL;
	all = NULL;
}
BiMRRR::BiMRRR() : BiSVD("BiMRRR NGN") {
	setDefaultSettings();
}
BiMRRR::BiMRRR(const char * name_) : BiSVD(name_) {
	setDefaultSettings();
}
struct BiMRRRTaskData {
	int32_t    n; // number of eigenvalues in cluster 
	int32_t	ind; // staring index of cluster
	bool isSingleton() const { return n == 1; };
	int  size()        const { return n; };
};

struct HeapData {
	double   val;
	int    index;
};
// given an array of N elements, find the smallest K of them, without need to
// be sorted
struct MinHeap {
	MinHeap(HeapData * RS storage) {
		data = storage; assert(data != NULL);
		n = 0;
	}
	void push(const double val, const int index) {
		data[n].val = val;
		data[n].index = index;
		int j = n++;
		int i = j >> 1;
		while ( j > 0 && data[j].val > data[i].val) {
			swap(i,j);
			j = i; i = i >> 1;
		}
	}
	void push_cmp(const double val, const int index) {
		if (val < data[0].val) {
			data[0].val   = val;
			data[0].index = index;
			int i = 0; int j = 1;
			j += (j < n) && data[j+1].val > data[j].val;
			while (j < n && data[j  ].val > data[i].val) {
				swap(i,j);
				i = j; j = i << 1;
				j += (j < n) &&  data[j+1].val > data[j].val;
			}

		}
	};
private:
	void swap(const int i, const int j) {
		HeapData tmp = data[i];
		data[i] = data[j]; 
		data[j] = tmp;
	}
	HeapData * data;
	int n;
};

void BiMRRR::checkBorders(const GK & gk, const int i) const {
	double left, right;
	all[i].getLeftRight(left, right);
	left  = gk.findEigenBorder(all[i].leftCount,  left,  all[i].width);		

	assert(gk.callCount(left) == all[i].leftCount);
	right = gk.findEigenBorder(all[i].rightCount, right, all[i].width);		

	assert(gk.callCount(right) == all[i].rightCount);
	
	all[i].center = (left + right)/2;
	all[i].width  = (right - left)  ;
	double newLeft, newRight;
	all[i].getLeftRight(newLeft, newRight);
	if (newLeft > left || newRight < right) {
		double max = fmax(-left, right);
		all[i].width  += 2 * DBL_EPSILON * max;
		all[i].getLeftRight(newLeft, newRight);
	}

	assert(newLeft <= left && newRight >= right);
	assert(gk.callCount(newRight) == all[i].rightCount);
	assert(gk.callCount(newLeft ) == all[i].leftCount);
	assert(left < right) ;
}
void BiMRRR::computeEnvelopeVector(const GK & left, const GK & right, double * RS s2) const {
	assert(left.isNGN() && right.isNGN());
	int n = left.getN();
	const double * RS  leftGamma = left.getData() + 8 * n;
	const double * RS rightGamma = right.getData() + 8 * n;
	for (int i = 0; i < 2 * n; i++)
		s2[i] = 1.0/leftGamma[i] - 1.0/rightGamma[i];
	
}

void BiMRRR::computeSingleton(const GK & parent, const int i) const{

	cout <<  __PRETTY_FUNCTION__  <<" line : " << __LINE__ << endl;
	double left, right;
	const int n = parent.getN();
	double z[2 * n];
	double minGamma = 0;
	int minIndex = 0; 
	double norm2 = 0, residual = 0; 
	double normUV[2];

	all[i].getLeftRight(left, right);
	checkBorders(parent, i);
	all[i].getLeftRight(left, right);

	double shiftFromParent = -all[i].center;
	double childNGNStorage[10 * n];
	GK child(NGN, parent, childNGNStorage, 10 * n, shiftFromParent);
	all[i].shift(shiftFromParent, 100 * n * DBL_EPSILON);
	
	child.getMinGamma(minIndex, minGamma);
	child.setRepIndex(minIndex);
	child.getEigen(z, 1, norm2, residual, normUV);
//	cout << "singleton " << i << " " << residual << " " << norm2 << " " << 
//	 normUV[0] << " " << normUV[1] << endl;
	double correction = -residual/norm2;
	int iter = 0;
	double final = -child.getShift();
	cout <<  __PRETTY_FUNCTION__  <<" line : " << __LINE__ << endl;
	if (fabs(correction) > fabs(1e-15 * child.getShift()) ) {
		double child2Storage[4 * n];
		GK child2(NGNPack, child, child2Storage, 4 * n, 0);
		cout <<  __PRETTY_FUNCTION__  <<" line : " << __LINE__ << endl;
		while (fabs(correction) > fabs(1e-15 * child.getShift()) && 
				iter++ < maxIter) {
			cout <<  __PRETTY_FUNCTION__  <<" line : " << __LINE__ << endl;
			child2 += correction;
			child2.getEigen(z, 1, norm2, residual, normUV);
			correction = -residual/norm2;
//			cout << "singleton " << i << " " << residual << " " << norm2 << " " << 
//			 normUV[0] << " " << normUV[1] << endl;
		}
		final = -child2.getShift() ;
//		cout << child2 << endl;

		// compute T q_i - lambda_i q_i
		/*
		double t[2 * n];
		double x[2*n];
		const double nrm = 1.0/sqrt(norm2);
		for (int j=0; j < 2 * n; j++) {
			t[j] /= nrm * z[j];
		}
		child2.getResidual(z,1,x,1);
		long double totRes2 = 0;
		for (int j = 0; j < 2 * n; j++) {
			cout << x[j] << " ";
			totRes2 += x[j] * x[j];
		}
		cout << endl;
		cout << "|T q_i - \\lambda q_i| = " << sqrt(totRes2) << endl;
		*/
	}

	cout <<  __PRETTY_FUNCTION__  <<" line : " << __LINE__ << endl;
	int vecP = n - i - 1;
	extractSingularVectors(z, u + vecP * jumpU, v + vecP * jumpV, norm2, n,
		parent.getSig());
	all[i].center = final;
	s[vecP] = all[i].center;
	cout <<  __PRETTY_FUNCTION__  <<" line : " << __LINE__ << endl;

}

void BiMRRR::computeGoodChild(const GK & parent, const int clusterStart, const int clusterStop, GK & child, double & shift) const {

	int n = parent.getN();
	int i = clusterStart;

	double leftShift = -(all[i].center - all[i].width/2); 
	int j = clusterStop - 1;
	j = i;
	while (all[j].rightCount != clusterStop + 1) {
		j = all[j].rightCount;
	}
	double rightShift = -(all[j].center + all[j].width/2); 

	double  leftNGNStorage[10 * n];
	double rightNGNStorage[10 * n];
	GK  leftChild(NGN, parent,  leftNGNStorage, 10 * n,  leftShift);
	GK rightChild(NGN, parent, rightNGNStorage, 10 * n, rightShift);
	double s2[2 * n]; // envelope vector

	computeEnvelopeVector(leftChild, rightChild, s2);
	double valLeft, valRight;

	 leftChild.chooseRepIndex(s2, valLeft );
	rightChild.chooseRepIndex(s2, valRight);

	if ( valRight < valLeft ) {
		child.shiftFrom(rightChild, 0);
		shift = rightShift;
	} else {
		child.shiftFrom( leftChild, 0);
		shift = leftShift;
	}
//	cout << "computed child " << clusterStart << ":" << clusterStop << child;

}
void BiMRRR::recallculateBorders(const GK & child, const int clusterStart, 
					const int clusterStop, const double shift	) const {

	double relAcc = refineRelAcc;

	int i = clusterStart;
	int n = child.getN();
	while ( i <= clusterStop) {
		// shift the interval to corespond to child rep
		all[i].shift(shift, 100 * n * DBL_EPSILON); 		
		checkBorders(child, i); // recalculate borders, if nessesery
		i = all[i].rightCount;  // next client ;) 
	}
	i = clusterStart;
	double left, right;
	while ( i <=  clusterStop ) {
		all[i].getLeftRight(left, right);
		while ( all[i].width > relAcc * fabs(all[i].center) &&
					left != all[i].center && right != all[i].center  ) {
			int countCenter = child.callCount(all[i].center);
			if ( countCenter == all[i].leftCount || 
				  countCenter == all[i].rightCount ) { 
				all[i].update(countCenter, left, right);
			} else {	// split the allval
				all[i].split(countCenter, left, right, all[countCenter]);
				if (all[i].count() == 1) { break;	}; 
			}
		}
		if ( all[i].count() == 1) 	{
			while ( all[i].width > relAcc * fabs(all[i].center) &&
					left != all[i].center && right != all[i].center ) {
				int countCenter = child.callCount(all[i].center);
				all[i].update(countCenter, left, right);
			}

		}
		i = all[i].rightCount; // go to next allvali
	}

}

void BiMRRR::computeCluster  (const GK & parent,  
		const int clusterStart, const int clusterStop) const {
	// assume that we alread refined enought usuing parrent
	int n = parent.getN();

	double childStorage[4 * n];
	GK child(NGNPack, n, childStorage, 4 * n);
	child.setNat(parent.getNat());
	child.setSig(parent.getSig());

	double relShift = 0;

	computeGoodChild   (parent, clusterStart, clusterStop, child, relShift);
	recallculateBorders(child , clusterStart, clusterStop,        relShift);


	int size = clusterStop - clusterStart + 1;
	int i = clusterStart;

	int newStartCluster = clusterStart;
	int newStopCluster = 0;
	int newSize = 0;

	while (i <= clusterStop) {
		int j = all[i].rightCount;
		if (j > clusterStop || !all[i].isClustered(gapTol, all[j])) {
			newStopCluster = all[i].rightCount - 1;
			newSize = newStopCluster - newStartCluster + 1;
			if ( newSize < size ) {	
				if (newStopCluster == newStartCluster) { // singleton found
					computeSingleton(child, newStopCluster);
				} else { // cluster found
					computeCluster(child, newStartCluster, newStopCluster);
				}
				newStartCluster = j;
			}
		}
		i = j;
	}

	if (newSize == size) {
		computeCluster(child, clusterStart, clusterStop);
	}
	

}

void BiMRRR::setForComputing(const Matrix & B, Matrix & U, Matrix & S, Matrix & V) {
	n = B.rows();
	all = new Interval[n]; memset(all, 0, n * sizeof(all[0]));
	nat = new GK(B, 0);
	bis = new GKBisector( *nat ) ;
	bis->setIntervalArray(all);
	bis->setRelAcc(getBisRelAcc());

	u = U.getModData();
	v = V.getModData();
	s = S.getModData();

	jumpU = U.isRowMajor() ? 1 : U.lda();
	jumpV = V.isColMajor() ? 1 : V.lda();
	incU  = jumpU == 1 ? U.lda() : 1;	
	incV  = jumpV == 1 ? V.lda() : 1;
	setExtractFunctionPointer();

}
void BiMRRR::doneComputing() {
	if (all) 
		for ( int i = 0; i < n; i++) {
			s[n - i -1] = all[i].center;
		}
	delArr(all);
	delObj(nat);
	delObj(bis);
	s = NULL;
	u = NULL;
	v = NULL;
}
void BiMRRR::compute(const Matrix & B, Matrix & U, Matrix & Sigma, Matrix & V){
	int n = B.rows();
	all = new Interval[n]; memset(all, 0, sizeof(all));
	// form native representation
	GK nat(B, 0);
	GKBisector bis( nat ) ;

	bis.setGapTol(gapTol);
	bis.setRelAcc(bisRelAcc);

	bis.setIntervalArray(all);
	bis.identifyClusters(all);

	u = U.getModData();
	v = V.getModData();
	s = Sigma.getModData();

	U.fill(0); V.fill(0);
	jumpU = U.isRowMajor() ? 1 : U.lda();
	jumpV = V.isColMajor() ? 1 : V.lda();
	incU  = jumpU == 1 ? U.lda() : 1;	
	incV  = jumpV == 1 ? V.lda() : 1;
	setExtractFunctionPointer();

	int i = 0;
	int startCluster = 0, stopCluster;
	while (i < n) {
		int j = all[i].rightCount;
		if (j == n || !all[i].isClustered(gapTol, all[j])) {
			stopCluster = all[i].rightCount - 1;
			if (stopCluster == startCluster) { // singleton found
				all[i].shift(0, n * 100 * DBL_EPSILON);
				computeSingleton(nat, startCluster);
			} else { // cluster found
				computeCluster(nat, startCluster, stopCluster);
			}
			startCluster = j;
		}
		i = j;
	}

	for ( int i = 0; i < n; i++) {
		s[n - i -1] = all[i].center;
	}
	delete [] all; all = NULL;
	u = NULL;
	v = NULL;
}
void BiMRRR::extractNoGap(BIMRRR_EXTRACT_PARAMS) const {
	const double rn[2] = { sqrt(2.0/norm2), -sqrt(2.0/norm2) };
	assert(sig != NULL);
	const int nBits = sizeof(int) * 4;
	const int kLimit = n/nBits;
	for (int k = 0; k < kLimit; k++) {
		unsigned int s = 1 << (sizeof(int) * 8 - 1);
		for (int i = k * nBits; i < (k+1)* nBits; i++) {
			bool c1 = (sig[k] & s) == 0; s = s >> 1;
			bool c2 = (sig[k] & s) == 0; s = s >> 1;
			v[i] = z[2 * i    ] * rn[c1]; 
			u[i] = z[2 * i + 1] * rn[c2]; 
		}
		assert(s == 0);
	}
//	cout.precision(16);
//	cout << "\n\n"<< kLimit << "\n";
	unsigned int s = 1 << (2 * (n - kLimit * nBits) - 1);
	for (int i = kLimit * nBits; i < n; i++) {
		bool c1 = (sig[kLimit] & s) == 0; s = s >> 1;
		bool c2 = (sig[kLimit] & s) == 0; s = s >> 1;
//		cout << c1 << " " << c2 << " ";
		v[i] = z[2 * i    ] * rn[c1]; 
		u[i] = z[2 * i + 1] * rn[c2]; 
	}
//	cout << "\nv = [";
//	for (int i = 0; i < n; i++) 
//		cout << v[i] << " "; cout << "];" << endl;
//	cout << "u = [";
//	for (int i = 0; i < n; i++) 
//		cout << u[i] << " "; cout << "];" << endl;
//	cout << "q = [";
//	for (int i = 0; i < 2 * n; i++) 
//		cout << z[i]/sqrt(norm2/2) << " "; cout << "];" << endl;
	
}
void BiMRRR::extractVGap (BIMRRR_EXTRACT_PARAMS) const {
//	cout << "\nBiMRRR::extractVGap" << endl;
	const double rn[2] = { sqrt(2.0/norm2), -sqrt(2.0/norm2) };
	assert(sig != NULL);
	const int nBits = sizeof(int) * 4;
	const int kLimit = n/nBits;
	for (int k = 0; k < kLimit; k++) {
		unsigned int s = 1 << (sizeof(int) * 8 - 1);
		for (int i = k * nBits; i < (k+1)* nBits; i++) {
			bool c1 = (sig[k] & s) == 0; s = s >> 1;
			bool c2 = (sig[k] & s) == 0; s = s >> 1;
			v[i * incV] = z[2 * i    ] * rn[c1]; 
			u[i       ] = z[2 * i + 1] * rn[c2]; 
		}
		assert(s == 0);
	}
	unsigned int s = 1 << (2 * (n - kLimit * nBits) - 1);
	for (int i = kLimit * nBits; i < n; i++) {
		bool c1 = (sig[kLimit] & s) == 0; s = s >> 1;
		bool c2 = (sig[kLimit] & s) == 0; s = s >> 1;
		v[i * incV] = z[2 * i    ] * rn[c1]; 
		u[i       ] = z[2 * i + 1] * rn[c2]; 
	}
	
}
void BiMRRR::extractUGap (BIMRRR_EXTRACT_PARAMS) const {
//	cout << "\nBiMRRR::extractUGap" << endl;
	const double rn[2] = { sqrt(2.0/norm2), -sqrt(2.0/norm2) };
	assert(sig != NULL);
	const int nBits = sizeof(int) * 4;
	const int kLimit = n/nBits;
	for (int k = 0; k < kLimit; k++) {
		unsigned int s = 1 << (sizeof(int) * 8 - 1);
		for (int i = k * nBits; i < (k+1)* nBits; i++) {
			bool c1 = (sig[k] & s) == 0; s = s >> 1;
			bool c2 = (sig[k] & s) == 0; s = s >> 1;
			v[i       ] = z[2 * i    ] * rn[c1]; 
			u[i * incU] = z[2 * i + 1] * rn[c2]; 
		}
		assert(s == 0);
	}

	unsigned int s = 1 << (2 * (n - kLimit * nBits) - 1);
	for (int i = kLimit * nBits; i < n; i++) {
		bool c1 = (sig[kLimit] & s) == 0; s = s >> 1;
		bool c2 = (sig[kLimit] & s) == 0; s = s >> 1;
		v[i       ] = z[2 * i    ] * rn[c1]; 
		u[i * incU] = z[2 * i + 1] * rn[c2]; 
	}
}
void BiMRRR::extractUVGap(BIMRRR_EXTRACT_PARAMS) const {
	cout << "\nBiMRRR::extractUVGap" << endl;
	const double rn[2] = { sqrt(2.0/norm2), -sqrt(2.0/norm2) };
	assert(sig != NULL);
	const int nBits = sizeof(int) * 4;
	const int kLimit = n/nBits;
	for (int k = 0; k < kLimit; k++) {
		unsigned int s = 1 << (sizeof(int) * 8 - 1);
		for (int i = k * nBits; i < (k+1)* nBits; i++) {
			bool c1 = (sig[k] & s) == 0; s = s >> 1;
			bool c2 = (sig[k] & s) == 0; s = s >> 1;
			v[i * incV] = z[2 * i    ] * rn[c1]; 
			u[i * incU] = z[2 * i + 1] * rn[c2]; 
		}
		assert(s == 0);
	}

	unsigned int s = 1 << (2 * (n - kLimit * nBits) - 1);
	for (int i = kLimit * nBits; i < n; i++) {
		bool c1 = (sig[kLimit] & s) == 0; s = s >> 1;
		bool c2 = (sig[kLimit] & s) == 0; s = s >> 1;
		v[i * incV] = z[2 * i    ] * rn[c1]; 
		u[i * incU] = z[2 * i + 1] * rn[c2]; 
	}
}
// PARALLEL BIMRRR
#include "Lock.h"
#include "Queue.h"
#include "MemoryPool.h"
#include "Task.h"
#include "TaskManager.h"

void BiMRRRPar::compute(const Matrix & B, Matrix & U, Matrix & S, Matrix & V) {
	setForComputing(B, U, S, V);
	TaskManager manager;
	manager.setN(getN());
	Task t;
	t.reset(Task::BiMRRRTask, dynamic_cast<BiMRRR*>(this), 0, -1);
	cout << B.rows() << " " << nThreads << endl;
	manager.perform(t, nThreads);
	doneComputing();
}


UNITTEST(TestPlayBiMRRR);
#include <iostream>
using namespace std;
TestPlayBiMRRR::TestPlayBiMRRR() {
	setName 				("Play with BiMRRR");
	setDescription  	("playing with different parts of BiMRRR");
	setTestAuthor		("Yordan Madzhunkov");
	setDeveloperAuthor("Yordan Madzhunkov");
	setCompileTime(__DATE__, __TIME__); // each class that inherits this one should also have this line.
};
bool TestPlayBiMRRR::test() const {
	int n = 10;
	double k = 1.0e-8 / 8;
	double d2 = 1e-8;
	double dHard [] = {1 + d2, 1-5 * d2, 4, 1 + 2 * d2, 1, 2 + d2, 2 + 2 * d2, 
		2 + 3 * d2, 3 + d2, 3 + 2 * d2, 
		k, 2 * k, 3 * k, 4 * k, 5 * k, 6 * k, 7 * k, 8 * k, 9 * k};
	Matrix B(n, n, BiDiagonal, dHard); assert(B.isUpper());
	Matrix U(n, n, Orthogonal);
	Matrix S(n, n, Diagonal);
	Matrix V(n, n, Orthogonal);

	BiMRRR bimrrr;
	bimrrr.compute(B,U,S,V);
	return (B * V.T()- U * S).nrm2() < 5.0 * ( n * DBL_EPSILON * B.nrm2());
}

UNITTEST(TestSignatureMatrix);
TestSignatureMatrix::TestSignatureMatrix() {
	setName 				("Test Signature matrix");
	setDescription  	("tests if signature matrix is correctly implemented");
	setTestAuthor		("Yordan Madzhunkov");
	setDeveloperAuthor("Yordan Madzhunkov");
	setCompileTime(__DATE__, __TIME__); // each class that inherits this one should also have this line.
};
bool TestSignatureMatrix::test() const {
	int n = 127;
	double * dHard = new double [2 * n];
	for (int i = 0; i < n; i++) 
		dHard[i] = (i & 1) == 1 ? i+1 : -(i+1);
	for (int i = n; i < 2 * n; i++) 
		dHard[i] = i % 3 == 2 ? (i+1.0)/10 : (i + 1.0) * 0.125;
	
	Matrix B(n, n, BiDiagonal, dHard); assert(B.isUpper());
	Matrix U(n, n, Orthogonal);
	Matrix S(n, n, Diagonal);
	Matrix V(n, n, Orthogonal);
	BiMRRR bimrrr;
	bimrrr.compute(B,U,S,V);
	delete [] dHard;
	return (B * V.T() - U * S).nrm2() < 5.0 * ( n * DBL_EPSILON * B.nrm2());
}

UNITTEST(TestBiMRRRWithGenerator);
TestBiMRRRWithGenerator::TestBiMRRRWithGenerator() {
	setName 				("Test BiMRRR with generator");
	setDescription  	("tests if signature matrix is correctly implemented");
	setTestAuthor		("Yordan Madzhunkov");
	setDeveloperAuthor("Yordan Madzhunkov");
	setCompileTime(__DATE__, __TIME__); // each class that inherits this one should also have this line.
};
bool TestBiMRRRWithGenerator::test() const {
	int n = 10;
	
	Matrix B(n, n, BiDiagonal); 
	Matrix U(n, n, Orthogonal); U.transpose();
	Matrix S(n, n, Diagonal);
	Matrix V(n, n, Orthogonal);

	MatrixGenerator generator;
	generator.setZeroProb  (0.0);
	BiMRRR bimrrr;
	int nConfigurations = 10;
	generator.generate(B);
	generator.generate(B);
//	B.getModOffDiagonal()[0] = fabs(B.getModOffDiagonal()[0]);
//	cout.precision(8);
/* 
	cout.precision(18);
	cout << "B = diag([";
	for (int i = 0; i < n; i++)
		cout << B.getData()[i] << " ";
	cout << "]) + diag([";
	for (int i = 0; i < n-1; i++)
		cout << B.getOffDiagonal()[i] << " ";
	cout << "],1);" << endl;

	cout << "T = diag([";
	for (int i = 0; i < n-1; i++) {
		cout << B.getData()[i] << " ";
		cout << B.getOffDiagonal()[i] << " ";
	}
	cout << B.getData()[n-1] << " ";
	cout << "],1);" << endl;
*/
			
	bimrrr.compute(B, U, S, V);
	double res = 0;
	getResidual(res, B, U, S, V);
	double orthU = U.OrthNorm();
	double orthV = V.OrthNorm();
	return res < 5;
}
TestPlayBiMRRR testPlayBiMRRR;
TestSignatureMatrix testSignatureMatrix;
TestBiMRRRWithGenerator testBiMRRRWithGenerator;



REGISTER(BiMRRR);
REGISTER(BiMRRRPar);
